/*
  SOLID - Interference Detection Library
  Copyright (C) 1997 Gino van den Bergen

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Library General Public
  License as published by the Free Software Foundation; either
  version 2 of the License, or (at your option) any later version.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Library General Public License for more details.

  You should have received a copy of the GNU Library General Public
  License along with this library; if not, write to the Free
  Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.

  Please send remarks, questions and bug reports to gino@win.tue.nl,
  or write to:
                  Gino van den Bergen
		  Department of Mathematics and Computing Science
		  Eindhoven University of Technology
		  P.O. Box 513, 5600 MB Eindhoven, The Netherlands
*/

#include "Polygon.h"
#include "Transform.h"
#include "PointBuf.h"

#include <algo.h>

const int MAXSIZE = 32;

#if defined(STATISTICS)
int poly_poly_count = 0;
int poly_interval_count = 0;
#endif

Polygon::Polygon(int npoints, const Point *points) : 
  verts(new Point[npoints]), nverts(npoints) 
{
  copy(&points[0], &points[npoints], &verts[0]);
  FILE *fp;
  fp=fopen("debug.txt", "a+");
  Vector normal(0, 0, 0);
  Point refpt(0, 0, 0);
  for (int i = 0, j = nverts-1; i < nverts; j = i++) {
    normal[X] += (verts[j][Y] - verts[i][Y]) * (verts[j][Z] + verts[i][Z]);
    normal[Y] += (verts[j][Z] - verts[i][Z]) * (verts[j][X] + verts[i][X]);
    fprintf(fp, "\nnormal[Z] - %.15f\t%.15f\n",(verts[j][X] - verts[i][X]),(verts[j][Y] + verts[i][Y]));
    normal[Z] += (verts[j][X] - verts[i][X]) * (verts[j][Y] + verts[i][Y]);
    fprintf(fp, "\nnormal - %.15f\t%.15f\t%.15f\n",normal[X],normal[Y],normal[Z]);
    refpt += verts[i];
  }
  fprintf(fp, "refpt - %g\t%g\t%g\n",refpt[X],refpt[Y],refpt[Z]);
  refpt /= nverts; 
  fprintf(fp, "refpt - %g\t%g\t%g\n",refpt[X],refpt[Y],refpt[Z]);
  fprintf(fp, "%.15f\t%.15f\t%.15f\t%.15f\t%.15f\t%.15f\t%.15f\t%.15f\t%.15f\t%.15f\t%.15f\t%.15f",verts[0][X],verts[0][Y],verts[0][Z],verts[1][X],verts[1][Y],verts[1][Z],verts[2][X],verts[2][Y],verts[2][Z],verts[3][X],verts[3][Y],verts[3][Z]);
  
  fclose(fp);
  
  plneq.setValue(normal, refpt);
}

Polygon::~Polygon() { 
  delete [] verts; 
}

Polygon::Polygon(const Polygon& x, const Transform& t) : 
  verts(new Point[x.nverts]), nverts(x.nverts), plneq(x.plneq, t) 
{ 
  transform(&x[0], &x[x.size()], &verts[0], t); 
}

bool Polygon::contains(const Point& p) const {
  int z = plneq.normal().closestAxis();
  int x = (z + 1) % 3;
  int y = (z + 2) % 3;
  
  bool inside = false;
  for (int i = 0, j = nverts-1; i < nverts; j = i++) {
    Point& u = verts[i];
    Point& v = verts[(i + 1) % nverts];
    if ((u[y] >= p[y]) != (v[y] >= p[y])) {
      bool right = u[x] >= p[x];
      if (right != (v[x] >= p[x])) {
	Scalar t = (u[y] - p[y]) / (u[y] - v[y]);
	if (u[x] + (v[x] - u[x]) * t >= p[x]) inside = !inside;
      }
      else if (right) inside = !inside;
    }
  }
  return inside;
}

void Polygon::interval(const Plane& plane, Scalar& b, Scalar& e) const {
#if defined(STATISTICS)
  ++poly_interval_count;
#endif
  b = e = plane(verts[0]);
  for (int i = 1; i < nverts; ++i) {
    Scalar d = plane(verts[i]);		 
    set_min(b, d);
    set_max(e, d);
  }
}

#if defined(EDGE_POLY)

inline bool edge_poly(const Polygon& poly1, const Polygon& poly2) {
  static Scalar dist[MAXSIZE];
  
  transform(&poly1[0], &poly1[poly1.size()], &dist[0], poly2.plane());
 
  int prev = poly1.size()-1;
  for (int i = 0; i < poly1.size(); ++i) {
    if ((dist[i] >= 0) != (dist[prev] >= 0)) {
      Scalar t = dist[i] / (dist[i] - dist[prev]);
      if (poly2.contains(poly1[i] + (poly1[prev] - poly1[i]) * t))
	return true;
    }    
    prev = i;
  }

  return false;
}


bool intersect(const Polygon& poly1, const Polygon& poly2) { 
     printf("intersezione tra poligoni 2");

#if defined(STATISTICS)
  ++poly_poly_count;
#endif

  return edge_poly(poly1, poly2) || edge_poly(poly2, poly1);
}

void intersect(const Polygon& poly1, const Polygon& poly2, PointBuf& report) {}

#else

int cut(const Polygon& poly, const Plane& plane, PointBuf& result) {
    // printf("intersezione tra poligoni 1\n");
    Plane plneq1 = (&poly)->plane();
    Point* verts1 = (&poly)->getVertex();
    Vector norm1 = (&plneq1)->normal();
    Scalar dist1 = (&plneq1)->distance();
    // printf("cut - poly\nnverts: %d\n",(&poly)->size());
    // printf("cut - verts: %g\t%g\t%g\n%g\t%g\t%g\n%g\t%g\t%g\n%g\t%g\t%g\n",verts1[0][0],verts1[0][1],verts1[0][2],verts1[1][0],verts1[1][1],verts1[1][2],verts1[2][0],verts1[2][1],verts1[2][2],verts1[3][0],verts1[3][1],verts1[3][2]);
    // printf("cut - plane.norm:\t%g\t%g\t%g\nplane.dist:\t%g\n",norm1[X],norm1[Y],norm1[Z],dist1);

    norm1 = (&plane)->normal();
    dist1 = (&plane)->distance();
    // printf("plane - plane.norm:\t%g\t%g\t%g\nplane.dist:\t%g\n",norm1[X],norm1[Y],norm1[Z],dist1);

  static Scalar dist[MAXSIZE];
  
  result.clear();
  
  transform(&poly[0], &poly[poly.size()], &dist[0], plane);

  int k = 0;
  // printf("dist - %g\t%g\t%g\t%g\t%g\t%g\n",dist[0],dist[1],dist[2],dist[3],dist[4],dist[5]);
 
  int prev = poly.size()-1;
  // printf("prev:\t%d\n",prev);
  for (int i = 0; i < poly.size(); ++i) {
      // printf("i:\t%d\n",i);
      // printf("prev:\t%d\n",prev);
      // printf("dist[i]:\t%g\n",dist[i]);
      // printf("dist[prev]:\t%g\n",dist[prev]);

    if ((dist[i] >= 0) != (dist[prev] >= 0)) {
      Scalar t = dist[i] / (dist[i] - dist[prev]);
      // printf("t:\t%g\n",t);
      Vector temp0 = (poly[prev] - poly[i]);
      Vector temp1 = (poly[prev] - poly[i]) * t;
      Vector temp2 = (poly[i] + (poly[prev] - poly[i]) * t);
      // Point* temp = &(temp0);
      // printf("cut - temp: %g\t%g\t%g\n",temp[X],temp[Y],temp[Z]);
      result.push_back(poly[i] + (poly[prev] - poly[i]) * t);
      
    }    
    prev = i;
  }
  // printf("result.size():\t%d\n",result.size());
  return result.size();
}

struct CompAxis {
  int axis;
  bool operator()(const Point& p1, const Point& p2) const {
    return p1[axis] < p2[axis];
  }
  CompAxis(int a) : axis(a) {}
} compAxis[3] = { X, Y, Z };

static PointBuf seg1, seg2;


bool intersect(const Polygon& poly1, const Polygon& poly2) { 
  FILE *fp;
  fp=fopen("intersect.txt", "a+");

    // printf("intersezione tra poligoni 1\n");
    Plane plneq1 = (&poly1)->plane();
    Point* verts1 = (&poly1)->getVertex();
    Vector norm1 = (&plneq1)->normal();
    Scalar dist1 = (&plneq1)->distance();
    fprintf(fp, "poly1\nnverts: %d\n",(&poly1)->size());
    fprintf(fp, "verts: %g\t%g\t%g\n%g\t%g\t%g\n%g\t%g\t%g\n%g\t%g\t%g\n",verts1[0][0],verts1[0][1],verts1[0][2],verts1[1][0],verts1[1][1],verts1[1][2],verts1[2][0],verts1[2][1],verts1[2][2],verts1[3][0],verts1[3][1],verts1[3][2]);
    fprintf(fp, "plane.norm:\t%g\t%g\t%g\nplane.dist:\t%g\n",norm1[X],norm1[Y],norm1[Z],dist1);

    Plane plneq2 = (&poly2)->plane();
    Point* verts2 = (&poly2)->getVertex();
    Vector norm2 = (&plneq2)->normal();
    Scalar dist2 = (&plneq2)->distance();
    fprintf(fp, "poly2\nnverts: %d\n",(&poly2)->size());
    fprintf(fp, "verts: %g\t%g\t%g\n%g\t%g\t%g\n%g\t%g\t%g\n%g\t%g\t%g\n",verts2[0][0],verts2[0][1],verts2[0][2],verts2[1][0],verts2[1][1],verts2[1][2],verts2[2][0],verts2[2][1],verts2[2][2],verts2[3][0],verts2[3][1],verts2[3][2]);
    fprintf(fp, "plane.norm:\t%g\t%g\t%g\nplane.dist:\t%g\n",norm2[X],norm2[Y],norm2[Z],dist2);
     
#if defined(STATISTICS)
  ++poly_poly_count;
#endif


  Vector v = cross(poly1.plane().normal(), poly2.plane().normal());
  // printf("v:\t%g\t%g\t%g\n",v[0],v[1],v[2]);
  if (!v.approxZero()) {
    if (cut(poly1, poly2.plane(), seg1) && cut(poly2, poly1.plane(), seg2)) {
      // printf("cut(poly1, poly2.plane(), seg1):\t%d\n",cut(poly1, poly2.plane(), seg1));
      // printf("cut(poly2, poly1.plane(), seg2):\t%d\n",cut(poly2, poly1.plane(), seg2));

      int axis = v.closestAxis();
      // printf("axis:\t%d\n",axis);

      int k = 0;
      // for (k = 0; k < seg1.size(); k++) printf("seg1 - %g\t%g\t%g\n",seg1[k][0],seg1[k][1],seg1[k][2]);
      // for (k = 0; k < seg1.size(); k++) printf("seg2 - %g\t%g\t%g\n",seg2[k][0],seg2[k][1],seg2[k][2]);

      sort(&seg1[0], &seg1[seg1.size()], compAxis[axis]);
      sort(&seg2[0], &seg2[seg2.size()], compAxis[axis]);
      
      // for (k = 0; k < seg1.size(); k++) printf("sort seg1 - %g\t%g\t%g\n",seg1[k][0],seg1[k][1],seg1[k][2]);
      // for (k = 0; k < seg1.size(); k++) printf("sort seg2 - %g\t%g\t%g\n",seg2[k][0],seg2[k][1],seg2[k][2]);

      int i = 0; 
      int j = 0;
      while (i < seg1.size() && j < seg2.size()) {
            // printf("seg1[i+1][axis]: %g\n",seg1[i+1][axis]);
            // printf("seg2[j][axis]: %g\n",seg2[j][axis]);
            // printf("seg2[j+1][axis]: %g\n",seg2[j+1][axis]);
            // printf("seg1[i][axis]: %g\n",seg1[i][axis]);
	if (seg1[i+1][axis] < seg2[j][axis]) {
                        i += 2;
                        // printf("seg1[i+1][axis] < seg2[j][axis]");
                        }
	else if (seg2[j+1][axis] < seg1[i][axis]) {
         j += 2;
                        // printf("seg2[j+1][axis] < seg1[i][axis]");
                        }
	else {
         fprintf(fp, "true\n");
  fclose(fp);
         return true;
      }
      }
    }
  }
         fprintf(fp, "false\n");
  fclose(fp);
  return false;
}

void intersect(const Polygon& poly1, const Polygon& poly2, PointBuf& report) {

#if defined(STATISTICS)
  ++poly_poly_count;
#endif

  Vector v = cross(poly1.plane().normal(), poly2.plane().normal());
  if (!v.approxZero()) {
    if (cut(poly1, poly2.plane(), seg1) && cut(poly2, poly1.plane(), seg2)) {
      int axis = v.closestAxis();

      sort(&seg1[0], &seg1[seg1.size()], compAxis[axis]);
      sort(&seg2[0], &seg2[seg2.size()], compAxis[axis]);
      
      int i = 0;
      int j = 0;
      while (i < seg1.size() && j < seg2.size()) {
	if (seg1[i+1][axis] < seg2[j][axis]) i += 2;
	else if (seg2[j+1][axis] < seg1[i][axis]) j += 2;
	else {
	  report.push_back(seg1[i][axis] < seg2[j][axis] ? seg2[j] : seg1[i]);
	  report.push_back(seg2[j+1][axis] < seg1[i+1][axis] ? seg2[j+1] : 
			   seg1[i+1]);
	  if (seg1[i+1][axis] < seg2[j+1][axis]) i += 2; else j += 2;
	}
      }
    }
  }
}

#endif

